import math

def adjust_speeds_enhanced(v1, v2, δ_R, d):
    """增强转向版本的速度调整"""
    S = abs(v1) + abs(v2)  # 速度绝对值之和
    if abs(v2 - v1) < 1e-6:
        R_curr = float('inf')
    else:
        R_curr = d * (v1 + v2) / (v2 - v1)
    
    R_new = R_curr + δ_R if R_curr != float('inf') else δ_R
    
    # 处理特殊情形
    if abs(R_new) > 1e6:  # 直行
        sign1 = 1 if v1 == 0 else v1/abs(v1)
        sign2 = 1 if v2 == 0 else v2/abs(v2)
        return (sign1*S/2, sign2*S/2)
    elif abs(R_new) < 1e-6:  # 原地旋转
        return (-S/2, S/2) if δ_R > 0 else (S/2, -S/2)
    
    # 常规计算
    p = (R_new - d)/(R_new + d)
    if p >= 0:  # 同向转向
        v2_new = math.copysign(S/(p + 1), v2)
    else:       # 反向转向
        v2_new = math.copysign(S/(abs(p) + 1), -1 if δ_R > 0 else 1)
    v1_new = p * v2_new
    
    return (v1_new, v2_new)

def run_enhanced_test(v1, v2, δ_R, d, case_name):
    """增强版测试函数"""
    print(f"\n【{case_name}】")
    print(f"初始速度: v1={v1:.2f}, v2={v2:.2f}")
    print(f"调整量 δ_R: {δ_R:.2f}")
    
    # 当前状态
    S_old = abs(v1) + abs(v2)
    R_curr = float('inf') if abs(v2-v1)<1e-6 else d*(v1+v2)/(v2-v1)
    
    # 执行调整
    v1_new, v2_new = adjust_speeds_enhanced(v1, v2, δ_R, d)
    
    # 新状态
    S_new = abs(v1_new) + abs(v2_new)
    R_new = float('inf') if abs(v2_new-v1_new)<1e-6 else d*(v1_new+v2_new)/(v2_new-v1_new)
    delta_actual = R_new - R_curr if (R_curr!=float('inf') and R_new!=float('inf')) else float('nan')
    
    # 验证条件
    radius_pass = (abs(delta_actual - δ_R) < 1e-6 if not math.isnan(delta_actual) 
                  else (abs(R_new - δ_R) < 1e-6 if R_curr == float('inf') else True))
    speed_sum_pass = abs(S_new - S_old) < 1e-6
    
    # 输出
    print(f"速度变化: ({v1:.2f}, {v2:.2f}) → ({v1_new:.2f}, {v2_new:.2f})")
    print(f"速度绝对值和: {S_old:.3f} → {S_new:.3f}")
    print(f"半径变化: {R_curr:.3f} → {R_new:.3f} (Δ={delta_actual:.3f})")
    print(f"测试结果: Radius={'PASS' if radius_pass else 'FAIL'}, SpeedSum={'PASS' if speed_sum_pass else 'FAIL'}")

# 系统参数
d = 0.5

# ===== 测试案例 =====
print("="*60 + "\n增强转向能力测试\n" + "="*60)

# 1. 直行 → 转弯
run_enhanced_test(1.0, 1.0, 0.5, d, "直行 → 左转 (R=0.5)")
run_enhanced_test(0.8, 0.8, -0.3, d, "直行 → 右转 (R=-0.3)")
run_enhanced_test(0.0, 0.0, 0.2, d, "静止 → 左转 (R=0.2)") 
run_enhanced_test(1.5, 1.5, -0.1, d, "高速直行 → 右转 (R=-0.1)")

# 2. 转弯 → 直行
run_enhanced_test(1.2, 0.8, 2.0, d, "右转 → 直行 (δ=2.0)")
run_enhanced_test(-0.6, 0.6, -0.5, d, "原地左转 → 右转 (δ=-0.5)")
run_enhanced_test(1.4, 0.6, 1.8, d, "右转 → 直行 (δ=1.8)")
run_enhanced_test(-1.0, 1.0, float('inf'), d, "原地左转 → 直行")

# 3. 直行 → 直行 
run_enhanced_test(1.0, 1.0, float('inf'), d, "直行 → 保持直行")
run_enhanced_test(-0.5, -0.5, 0.0, d, "反向直行 → 保持")
run_enhanced_test(0.0, 0.0, float('inf'), d, "静止 → 保持直行")
run_enhanced_test(2.0, 2.0, 0.0, d, "高速直行 → 保持")

# 4. 转向变急
run_enhanced_test(1.1, 0.9, -0.3, d, "右转增强 (δ=-0.3)")
run_enhanced_test(-0.7, 0.7, 0.2, d, "左转增强 (δ=0.2)")
run_enhanced_test(1.3, 0.7, -0.5, d, "右转增强 (δ=-0.5)") 
run_enhanced_test(-0.4, 0.6, 0.1, d, "左转增强 (δ=0.1)")

# 5. 转向变缓
run_enhanced_test(1.3, 0.7, 0.5, d, "右转减弱 (δ=0.5)")
run_enhanced_test(-0.8, 0.8, -0.2, d, "左转减弱 (δ=-0.2)")
run_enhanced_test(1.4, 0.6, 0.8, d, "右转减弱 (δ=0.8)")
run_enhanced_test(-0.9, 0.9, -0.3, d, "左转减弱 (δ=-0.3)")